﻿using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Media.Media3D;
using Microsoft.Kinect;

namespace FeatureDetection.Inputter {
    //TODO KinectInputter未実装
    class KinectInputter : IInputter{
        private KinectSensor sensor;
        private IInputter.AddPosition addpos;

        public override bool initialize() {
            foreach(var potentialSensor in KinectSensor.KinectSensors) {
                if(potentialSensor.Status == KinectStatus.Connected) {
                    this.sensor = potentialSensor;
                    return true;
                }
            }
            return false;
        }

        public override void startInput(IInputter.AddPosition addpos, IInputter.EndFunc endFunc) {
            this.addpos = addpos;
            if(null != this.sensor) {
                this.sensor.SkeletonStream.TrackingMode = SkeletonTrackingMode.Seated;
                this.sensor.SkeletonStream.Enable();
                this.sensor.SkeletonFrameReady += this.SensorSkeletonFrameReady;

                try {
                    this.sensor.Start();
                } catch(IOException) {
                    this.sensor = null;
                }
            }
        }

        private void SensorSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e) {
            Skeleton[] skeletons = new Skeleton[0];

            using(SkeletonFrame skeletonFrame = e.OpenSkeletonFrame()) {
                if(skeletonFrame != null) {
                    skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
                    skeletonFrame.CopySkeletonDataTo(skeletons);
                }
            }

            if(skeletons.Length != 0) {
                //一番近い人を選択
                float depth = float.MaxValue;
                int index = 0;
                for(int i = 0; i < skeletons.Length; ++i) {
                    if(skeletons[i].Position.Z < depth && skeletons[i].TrackingState == SkeletonTrackingState.Tracked) {
                        index = i;
                        depth = skeletons[i].Position.Z;
                    }
                }

                Skeleton skel = skeletons[index];
                if(skel.TrackingState == SkeletonTrackingState.Tracked) {
                    var p = SkeletonPointToScreen(skel.Joints[JointType.HandRight].Position);
                    addpos(p);
                }
            }
        }

        private Vector3D SkeletonPointToScreen(SkeletonPoint skelpoint) {
            DepthImagePoint depthPoint = this.sensor.MapSkeletonPointToDepth(
                                                                             skelpoint,
                                                                             DepthImageFormat.Resolution640x480Fps30);
            return new Vector3D(depthPoint.X, depthPoint.Y,depthPoint.Depth);
        }

        public override void endInput() {
            if(null != this.sensor) this.sensor.Stop();
        }
    }
}
